import os
import sys
import rospy
from geometry_msgs.msg import PoseStamped
import math
import itertools
import numpy as np
import astar

 
if __name__ == '__main__':
    gx=10
    gy=90
    ox=10
    oy=35
    rospy.init_node("goal_pub")
    pub_goal = rospy.Publisher('goal_pose',PoseStamped,queue_size=10)
    pub_ob = rospy.Publisher('obstacle_pose',PoseStamped,queue_size=10)
    goal_pos=PoseStamped()
    obstacle_pos=PoseStamped()
    goal_pos.pose.position.x=gx
    goal_pos.pose.position.y=gy
    obstacle_pos.pose.position.x=ox
    obstacle_pos.pose.position.y=oy
    count = 0  #计数器 
    # 设置循环频率
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():

        if count>1:
            goal_pos.pose.position.x = goal_pos.pose.position.x +0.0
            pub_goal.publish(goal_pos)
            rospy.loginfo("目标的数据:%s",goal_pos.pose.position.x)
            obstacle_pos.pose.position.x = obstacle_pos.pose.position.x -0
            obstacle_pos.pose.position.y = obstacle_pos.pose.position.y +0
            pub_ob.publish(obstacle_pos)
            rospy.loginfo("障碍的数据:%s",obstacle_pos.pose.position.x)
        count += 1
        rate.sleep()
    

    # 测试区域
    # a=[]
    # b=[]
    # ox,oy=astar.get_env()
    # num=np.float64(3.324)
    # num=float(num)
    # a,b=astar.astar_planning(num, 10, 30, 50, ox, oy, 1 ,2)
    # c=np.array([1,2,3])
    # d=np.array([2,2,2])
    # r=np.sqrt(np.square(c)+np.square(d))
    # print(np.square(c)+np.square(d))
    # print(r)
    # x0=a[:-1]
    # x1=a[1:]
    
    # dis_x=np.array(x1)-np.array(x0)
    # y0=b[:-1]
    # y1=b[1:]
    # dis_y=np.array(y1)-np.array(y0)
    # dist=np.sqrt(np.square(dis_x)+np.square(dis_y))
    # k= 10/sum(dist)
    # sub_ts=list(itertools.accumulate(dist*k))
    # ts=[0]+sub_ts
    # print(ts)
    # a=np.array([[0,0,2]])
    # b=np.array([[10,90,2]])
    # c=np.concatenate((a,b),axis=0)
    # d=a[-1,1]
    # print(d)
